﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.ExportToRTR.CApExportToRTRRobotSupplementExternalAxisBase
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using DnProcessSimulateCommands.ExportToRTR.YAML;
using EngineeringInternalExtension;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.Linq;
using System.Text;
using System.Text.RegularExpressions;
using Tecnomatix.Engineering;
using Tecnomatix.Engineering.PrivateImplementationDetails;


namespace DnProcessSimulateCommands.ExportToRTR{

public abstract class CApExportToRTRRobotSupplementExternalAxisBase : CApExportToRTRRobotSupplement
{
  protected ITxDevice m_device;
  private int m_jointIndex;
  private List<ITxObject> m_nonCollidableEntities;
  protected List<ITxObject> m_jointsToExport;
  protected List<ITxObject> m_baseLinks;
  protected List<ITxObject> m_childLinks;

  public CApExportToRTRRobotSupplementExternalAxisBase(
    ITxDevice device,
    ITxLocatableObject attachmentParent,
    TxRobot robot,
    string root)
    : base(device as ITxLocatableObject, attachmentParent, robot, root)
  {
    this.m_device = device;
    this.m_jointIndex = ((Collection<ITxObject>) this.m_robot.DrivingJoints).Count;
    this.m_nonCollidableEntities = this.m_device is ITxTool ? ((IEnumerable<ITxObject>) (this.m_device as ITxTool).NonCollidingEntities).ToList<ITxObject>() : (List<ITxObject>) null;
    this.m_jointsToExport = this.GetJointsToExport();
    this.m_childLinks = new List<ITxObject>();
    this.m_jointsToExport.ForEach((Action<ITxObject>) (x => this.m_childLinks.Add((ITxObject) (x as TxJoint).ChildLink)));
    this.m_baseLinks = ((IEnumerable<ITxObject>) this.m_device.Links).ToList<ITxObject>().Except<ITxObject>((IEnumerable<ITxObject>) this.m_childLinks).ToList<ITxObject>();
  }

  protected abstract List<ITxObject> GetJointsToExport();

  public override void Export()
  {
    if (!Directory.Exists(Path.Combine(this.m_root, this.m_directory)))
      Directory.CreateDirectory(Path.Combine(this.m_root, this.m_directory));
    using (TxPoseData currentPose = this.m_device.CurrentPose)
    {
      ((IEnumerable<ITxObject>) this.m_device.DrivingJoints).ToList<ITxObject>().FindAll((Predicate<ITxObject>) (x => ((Collection<ITxObject>) this.m_robot.ExternalAxes).Contains(x))).ForEach((Action<ITxObject>) (x => (x as TxJoint).CurrentValue = 0.0));
      this.m_childLinks.ForEach((Action<ITxObject>) (x => ((IEnumerable<ITxObject>) (x as TxKinematicLink).LinkApprox()).ToList<ITxObject>().ForEach((Action<ITxObject>) (y => (y as ITxDisplayableObject).Blank()))));
      if (this.Export(this.m_device as ITxLocatableObject) != "emptyLink")
        this.ObjectsWith3D.Add((ITxObject) this.m_device);
      if (TxFeatureManager.CheckFeature("RTR_SMART_GUN_TIPS"))
        this.ExportBaseLinkGunTip();
      this.IgnoreBaseLink();
      this.ExportJointsWithChildLinks();
      this.m_device.CurrentPose = currentPose;
    }
  }

  private void IgnoreBaseLink()
  {
    foreach (ITxObject baseLink1 in this.m_baseLinks)
    {
      ITxObject baseLink = baseLink1;
      if (((IEnumerable<ITxObject>) this.m_device.AllJointsAfterCompilation).Any<ITxObject>((Func<ITxObject, bool>) (x => TxKinematicLink.op_Equality((x as TxJoint).ParentLink, baseLink))) && (!CApExportToRTRUtils.IsEmptyLink(baseLink as TxKinematicLink) || TxUnitedCollectionEx.DoesCollectionContainApprox((ITxObject) this.m_device)))
      {
        CApExportToRTRYamlMgr.Instance.m_Tx2YamlObject[baseLink] = CApExportToRTRYamlMgr.Instance.GetObj((ITxObject) this.m_device);
        this.AddLinkToIgnoreList(string.Empty, baseLink);
      }
    }
  }

  protected void ExportJointsWithChildLinks()
  {
    List<TxJoint> txJointList = new List<TxJoint>();
    List<ITxObject> all = ((IEnumerable<ITxObject>) this.m_robot.ExternalAxes).ToList<ITxObject>().FindAll((Predicate<ITxObject>) (x => (x as TxJoint).Device == this.m_device));
    foreach (ITxObject joint in this.m_jointsToExport)
      this.ExportChildLink(joint as TxJoint);
    foreach (ITxObject joint in all)
      this.CreateJoint(joint as TxJoint, true);
    foreach (ITxObject joint in this.m_jointsToExport)
    {
      if (!all.Contains(joint))
        this.CreateJoint(joint as TxJoint, false);
    }
  }

  private void AddExternalJoint(TxJoint externalJoint, int index)
  {
    if (!CApExportToRTRYamlMgr.Instance.ExternalJoints.ContainsKey(this.m_robot))
      CApExportToRTRYamlMgr.Instance.ExternalJoints.Add(this.m_robot, new TxJoint[((Collection<ITxObject>) this.m_robot.ExternalAxes).Count]);
    CApExportToRTRYamlMgr.Instance.ExternalJoints[this.m_robot][index] = externalJoint;
  }

  private void ExportChildLink(TxJoint joint)
  {
    ((IEnumerable<ITxObject>) joint.ChildLink.LinkApprox()).ToList<ITxObject>().ForEach((Action<ITxObject>) (y => (y as ITxDisplayableObject).Display()));
    this.m_nonCollidableEntities?.ForEach((Action<ITxObject>) (x => (x as ITxDisplayableObject).Blank()));
    this.CreateChildLink(joint);
    if (!TxFeatureManager.CheckFeature("RTR_SMART_GUN_TIPS"))
      return;
    this.ExportChildLinkGunTip(joint.ChildLink);
  }

  private void ExportBaseLinkGunTip()
  {
    if (this.m_nonCollidableEntities == null)
      return;
    ITxObject collection = (ITxObject) null;
    ITxObject gunTip = (ITxObject) null;
    for (int index = 0; index < this.m_nonCollidableEntities.Count && collection == null; ++index)
    {
      gunTip = this.m_nonCollidableEntities[index];
      ITxObject itxObject = gunTip;
      do
      {
        itxObject = (ITxObject) itxObject.Collection;
      }
      while (!(itxObject is TxKinematicLink) && itxObject != this.m_device);
      collection = this.m_childLinks.Contains(itxObject) ? (ITxObject) null : itxObject;
    }
    if (collection == null)
      return;
    this.ExportGunTip(gunTip, collection as ITxLocatableObject);
  }

  private void ExportChildLinkGunTip(TxKinematicLink kinematicLink)
  {
    ITxObject gunTip = this.m_nonCollidableEntities?.Find((Predicate<ITxObject>) (x => x.Collection == kinematicLink));
    if (gunTip == null)
      return;
    this.ExportGunTip(gunTip, (ITxLocatableObject) kinematicLink);
  }

  private void ExportGunTip(ITxObject gunTip, ITxLocatableObject collection)
  {
    if (collection is TxKinematicLink)
      ((IEnumerable<ITxObject>) (collection as TxKinematicLink).LinkApprox()).ToList<ITxObject>().ForEach((Action<ITxObject>) (y => (y as ITxDisplayableObject).Blank()));
    else
      (collection as ITxDisplayableObject).Blank();
    (gunTip as ITxDisplayableObject).Display();
    string cadFileName = this.ExportCad(collection, gunTip as ITxLocatableObject);
    if (cadFileName != "emptyLink")
      this.ChildrenWith3D.Add(gunTip);
    ObjectInstance objectInstance = this.ExportObject(gunTip as ITxLocatableObject, cadFileName);
    TxTransformation origin = CApExportToRTRRobotSupplement.GetOrigin(gunTip as ITxLocatableObject, collection, (ITxObjectCollection) this.m_robot);
    CApExportToRTRYamlMgr.Instance.AddObj<Mate>(new Mate()
    {
      Name = CApExportToRTRYamlUtils.GetMateUniqueName((ITxObject) collection, gunTip),
      Parent = new ObjectRef()
      {
        Object = this.GetParent(collection as ITxObjectCollection)?.Name
      },
      Child = objectInstance.CreateObjectRef(),
      OffsetXyz = new Vec3D(origin.Translation),
      OffsetRpy = new Vec3D(origin.GetRotation())
    });
    if (collection is TxKinematicLink)
      ((IEnumerable<ITxObject>) (collection as TxKinematicLink).LinkApprox()).ToList<ITxObject>().ForEach((Action<ITxObject>) (y => (y as ITxDisplayableObject).Display()));
    else
      (collection as ITxDisplayableObject).Display();
  }

  public string GetRtrFunctionFormat(string function)
  {
    StringBuilder stringBuilder = new StringBuilder();
    int num1 = 0;
    while (num1 < function.Length)
    {
      if ((function[num1] == 'D' || function[num1] == 'T') && function[num1 + 1] == '(')
      {
        int num2 = function.IndexOf(')', num1);
        string str = function.Substring(num1 + 2, num2 - num1 - 2);
        stringBuilder.Append($"(@{TxObjectEx.GetUniqueNameForRTR((ITxObject) this.m_device, true)}_{str}");
        num1 = num2;
      }
      else
      {
        stringBuilder.Append(function[num1]);
        ++num1;
      }
    }
    return Regex.Replace(stringBuilder.ToString(), "(?<=(?:(?<!\\w+)\\d+|\\)|(?:@\\w+(?!\\w+))))(?=(?:(?<!\\w)\\d|\\(|(?:(?<!\\d+)\\w+)))", "*");
  }

  private void CreateMate(string jointName, Frame parentFrame, Frame childFrame)
  {
    CApExportToRTRYamlMgr.Instance.AddObj<Mate>(new Mate()
    {
      Name = jointName,
      Parent = new ObjectRef() { Frame = parentFrame.Name },
      Child = new ObjectRef() { Frame = childFrame.Name },
      OffsetXyz = new Vec3D(new TxVector(0.0, 0.0, 0.0)),
      OffsetRpy = new Vec3D(new TxVector(0.0, Math.PI, Math.PI))
    });
  }

  private void CreateJoint(
    TxJoint joint,
    string jointName,
    Frame parentFrame,
    Frame childFrame,
    TxVector jointAxis)
  {
    Joint yamlObj = new Joint()
    {
      Name = jointName,
      Parent = parentFrame.CreateObjectRef(),
      Child = childFrame.CreateObjectRef(),
      Type = CApExportToRTRRobotPrototype.GetJointType(joint),
      OffsetXyz = new Vec3D(new TxVector(0.0, 0.0, 0.0)),
      OffsetRpy = new Vec3D(new TxVector(0.0, Math.PI, Math.PI)),
      Axis = new Vec3D(jointAxis)
    };
    if (!joint.IsDependent)
    {
      yamlObj.Limits = new JointLimits()
      {
        MinPosition = new double?(joint.LowerSoftLimit),
        MaxPosition = new double?(joint.UpperSoftLimit),
        MaxVelocity = new double?(joint.MaxSpeed),
        MaxAcceleration = new double?(joint.MaxAcceleration),
        MaxJerk = new double?(100000.0)
      };
      int index = ((IEnumerable<ITxObject>) this.m_robot.ExternalAxes).ToList<ITxObject>().FindIndex((Predicate<ITxObject>) (x => x == joint));
      yamlObj.DriverIndex = new int?(this.m_jointIndex + index);
      this.AddExternalJoint(joint, index);
    }
    else
      yamlObj.jointDependency = new JointDependency()
      {
        Function = this.GetRtrFunctionFormat(joint.KinematicsFunction)
      };
    CApExportToRTRYamlMgr.Instance.AddObj<Joint>(yamlObj, (ITxObject) joint);
  }

  private void CreateJoint(TxJoint joint, bool isExternalJoint)
  {
    TxVector jointFrom;
    TxVector jointTo;
    CApExportToRTRRobotPrototype.GetAxisPointsInDeviceCoordinates(joint, out jointFrom, out jointTo);
    TxTransformation txTransformation = CApExportToRTRRobotPrototype.CalcJointFrame(jointFrom, jointTo);
    TxVector jointAxis = this.GetJointAxis(txTransformation, jointFrom, jointTo);
    string jointName = $"{TxObjectEx.GetUniqueNameForRTR((ITxObject) this.m_device, true)}_{joint.Name}";
    Frame frame1 = new Frame()
    {
      Name = jointName + "_parent",
      Parent = new ObjectRef()
      {
        Object = this.GetParent(joint.ParentLink)?.Name
      },
      OffsetXyz = new Vec3D(txTransformation.Translation),
      OffsetRpy = new Vec3D(txTransformation.GetRotation())
    };
    CApExportToRTRYamlMgr.Instance.AddObj<Frame>(frame1);
    Frame frame2 = new Frame()
    {
      Name = jointName + "_child",
      Parent = new ObjectRef()
      {
        Object = CApExportToRTRYamlMgr.Instance.GetObj((ITxObject) joint.ChildLink).Name
      },
      OffsetXyz = new Vec3D(txTransformation.Translation),
      OffsetRpy = new Vec3D(txTransformation.GetRotation())
    };
    CApExportToRTRYamlMgr.Instance.AddObj<Frame>(frame2);
    if (joint.IsDependent | isExternalJoint)
      this.CreateJoint(joint, jointName, frame1, frame2, jointAxis);
    else
      this.CreateMate(jointName, frame1, frame2);
  }

  private TxVector GetJointAxis(TxTransformation jointFrame, TxVector jointFrom, TxVector jointTo)
  {
    TxVector txVector = CApExportToRTRRobotPrototype.CalcJointDirection(jointFrom, jointTo);
    return jointFrame.TransformNormal(txVector);
  }

  private IObj GetParent(TxKinematicLink link)
  {
    return this.m_baseLinks.Contains((ITxObject) link) ? CApExportToRTRYamlMgr.Instance.GetObj((ITxObject) this.m_device) : CApExportToRTRYamlMgr.Instance.GetObj((ITxObject) link);
  }

  private IObj GetParent(ITxObjectCollection collection)
  {
    return !(collection is TxKinematicLink) ? CApExportToRTRYamlMgr.Instance.GetObj((ITxObject) collection) : this.GetParent(collection as TxKinematicLink);
  }

  private IObj CreateChildLink(TxJoint joint) => (IObj) this.CreateLink(joint.ChildLink);

  private void AddLinkToIgnoreList(string cadFileName, ITxObject kinematicLink)
  {
    if (!CApExportToRTRYamlMgr.Instance.IgnoredObjects.ContainsKey(this.m_robot))
      CApExportToRTRYamlMgr.Instance.IgnoredObjects.Add(this.m_robot, new Tuple<List<ITxObject>, List<ITxObject>>(new List<ITxObject>(), new List<ITxObject>()));
    if (!(cadFileName != "emptyLink"))
      return;
    CApExportToRTRYamlMgr.Instance.IgnoredObjects[this.m_robot].Item2.Add(kinematicLink);
  }

  private ObjectInstance CreateLink(TxKinematicLink kinematicLink)
  {
    string cadFileName = this.ExportCad((ITxLocatableObject) kinematicLink);
    if (cadFileName != "emptyLink")
      this.ChildrenWith3D.Add((ITxObject) kinematicLink);
    ObjectInstance link = this.ExportObject((ITxLocatableObject) kinematicLink, cadFileName);
    this.AddLinkToIgnoreList(cadFileName, (ITxObject) kinematicLink);
    return link;
  }
}
